
#include "ContourCornerFinder.h"
#include "MiscFunctions.h"

#include <stdio.h>

using namespace cv;
using namespace std;
using namespace idch;

ContourCornerFinder::ContourCornerFinder( int width , double minSlope )
{
    this->radius = width/2;
    if( radius < 1 )
        radius = 1;

    minHeightSq = minSlope*radius;
    // distance is squared
    minHeightSq = minHeightSq*minHeightSq;
}

void ContourCornerFinder::find( const std::vector<cv::Point> &contour )
{
    corners.clear();

    // don't process the shape if its too small
    if( contour.size() < radius*2+1 )
        return;

    // stores how corner like a point is
    double intensity[ contour.size() ];

    for( size_t i = 0; i < contour.size(); ++i ) {
        intensity[i] = computeIntensity( contour , i );
    }

    // find local minimum
    int N = contour.size();
    for( size_t i = 0; i < contour.size(); ++i ) {
//        printf("%5.2f ",intensity[i]);
        if( isLocalMin( intensity , N , i ) ) {
            const cv::Point &pt = contour[ i ];

            corners.push_back(ContourCorner(i,pt.x,pt.y,0));
        }
    }
//    printf("\n");
}

const std::vector<ContourCorner>& ContourCornerFinder::getCorners() const
{
    return corners;
}

double ContourCornerFinder::
        computeIntensity( const std::vector<cv::Point> &contour , int target )
{
    int start = target - radius;
    int stop = target + radius;

    size_t N = contour.size();

    const cv::Point &a = start >= 0 ? contour.at(start) : contour.at(N+start);
    const cv::Point &b = stop < N ? contour.at(stop) : contour.at(stop-N);
    const cv::Point &pt = contour.at(target);

    return lineDistance2(a,b,pt);
}

bool ContourCornerFinder::isLocalMin( double *intensity , int N , int target )
{
    int start = target - radius;
    int stop = target + radius;

    double targetVal = intensity[target];

    if( start < 0 ) {
        start = N+start;
        for( int i = start; i < N; i++ ) {
            if( i == target ) continue;
            double v = intensity[i];
            if( v > targetVal ) {
                return false;
            }
        }
        start = 0;
    } else if( stop > N ) {
        stop = stop-N;
        for( int i = 0; i < stop; i++ ) {
            if( i == target ) continue;
            double v = intensity[i];
            if( v > targetVal ) {
                return false;
            }
        }
        stop = N;
    }
    for( int i = start; i < stop; i++ ) {
        if( i == target ) continue;
        double v = intensity[i];
        if( v > targetVal ) {
            return false;
        }
    }

    return targetVal > minHeightSq;
}
